package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.number.approx
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.ext.dot


/**
 * Performs a spherical-linear interpolation with another quaternion.
 */
fun<T:Number> Quaternion<T, *>.slerp(q: Quaternion<T, *>, t: T): Quaternion<T, *> {
    val to1 : Quaternion<T, *>
    val omega: T
    var cosom: T
    val sinom: T
    val scale0: T
    val scale1: T

    cosom = dot(q)

    if (cosom < 0) {
        cosom = -cosom
        to1 = create(-q.x, -q.y, -q.z, -q.w) as Quaternion<T, *>
        /*to1.x = -q.x
        to1.y = -q.y
        to1.z = -q.z
        to1.w = -q.w*/
    } else {
        /*to1.x = q.x*/
        /*to1.y = q.y*/
        /*to1.z = q.z*/
        /*to1.w = q.w*/
        to1 = create(q.x, q.y, q.z, q.w) as Quaternion<T, *>
    }
   //if ((1.0 - cosom) > CMP_EPSILON)
    if (!(1.0  approx cosom)) {
        // standard case (slerp)
        omega = acos(cosom)
        sinom = sin(omega)
        scale0 = (sin((1.0 - t) * omega) / sinom)
        scale1 = (sin(t * omega) / sinom)
    } else {
        // "from" and "to" quaternions are very close
        //  ... so we can do a linear interpolation
        scale0 = 1.0 - t + zero
        scale1 = t + zero
    }
    // calculate final values
    return create(
        scale0 * x + scale1 * to1.x,
        scale0 * y + scale1 * to1.y,
        scale0 * z + scale1 * to1.z,
        scale0 * w + scale1 * to1.w
    ) as Quaternion<T, *>
}

/**
 * Performs a spherical-linear interpolation with another quaterion without checking if the rotation path is not bigger than 90°.
 */
fun<T:Number> Quaternion<T, *>.slerpni(q: Quaternion<T, *>, t: Double): Quaternion<T, *> {
    val from = this
    val dot = from.dot(q)

    if (abs(dot) > 0.9999) return from

    val theta = acos(dot)
    val sinT = 1.0 / sin(theta)
    val newFactor = sin(t * theta) * sinT
    val invFactor= sin((1.0 - t) * theta) * sinT

    return create(
        invFactor * from.x + newFactor * q.x,
        invFactor * from.y + newFactor * q.y,
        invFactor * from.z + newFactor * q.z,
        invFactor * from.w + newFactor * q.w
    )  as Quaternion<T, *>
}
